#include "encoder.h"
#include <hardware/as5600.h>
#include <hardware/mt6701.h>
#include "foc_utils.h"
#include "time_utils.h"

inline float abs(float _v)
{
    return _v >= 0 ? _v : -_v;
}

Encoder::Encoder(EncoderDevice device) :
        device_(device)
{
    velocity_ = 0.0f;

    angle_prev_ = 0.0f;
    angle_prev_ts_ = 0;
    vel_angle_prev_ = 0.0f;
    vel_angle_prev_ts_ = 0;
    full_rotations_ = 0;
    vel_full_rotations_ = 0;
}

Encoder::~Encoder()
{
}

// 更新角度（弧度）
void Encoder::update()
{
    float val = getSensorAngle();
    angle_prev_ts_ = micros();
    float d_angle = val - angle_prev_;
    // if overflow happened track it as full rotation
    if (abs(d_angle) > (0.8f * _2PI))
        full_rotations_ += (d_angle > 0) ? -1 : 1;

    angle_prev_ = val;
}

// 获取速度（rad/s）
float Encoder::getVelocity()
{
    // calculate sample time
    float Ts = (float) (angle_prev_ts_ - vel_angle_prev_ts_) * 1e-6f;
    // quick fix for strange cases (micros overflow)
    if (Ts <= 0)
        Ts = 1e-3f;
    // velocity calculation
    float vel = ((float) (full_rotations_ - vel_full_rotations_) * _2PI + (angle_prev_ - vel_angle_prev_)) / Ts;
    // save variables for future pass
    vel_angle_prev_ = angle_prev_;
    vel_full_rotations_ = full_rotations_;
    vel_angle_prev_ts_ = angle_prev_ts_;
    return vel;
}

// 初始化参数
void Encoder::init()
{
    // initialize all the internal variables of EncoderBase to ensure a "smooth" startup (without a 'jump' from zero)
    getSensorAngle(); // call once
    delayMicroseconds(1);
    vel_angle_prev_ = getSensorAngle(); // call again
    vel_angle_prev_ts_ = micros();

    delayMillisecond(1);

    getSensorAngle(); // call once
    delayMicroseconds(1);
    angle_prev_ = getSensorAngle(); // call again
    angle_prev_ts_ = micros();
}

float Encoder::getMechanicalAngle()
{
    return angle_prev_;
}

float Encoder::getAngle()
{
    return (float) full_rotations_ * _2PI + angle_prev_;
}

//int32_t Encoder::getFullRotations()
//{
//    return full_rotations;
//}

float Encoder::getSensorAngle()
{
    if (device_ == MT6701)   // iic
    {
        return mt6701_getAngle();
    }
    else if (device_ == AS5600)  // iic
    {
        return as5600_getAngle();
    }
    return 0;
}
